#include "WPILib.h"
#include "Robot2489.h"

void Robot2489::UpdateGearToothVelocity(){

	if(newInterval){
		tIntial = timer->Get();
		xIntial = gearToothCounter->Get();
		newInterval = 0;
	}else{
		double currentTime;
		currentTime = timer->Get();
		double checkInterval;
		checkInterval = currentTime - tIntial;
		if(checkInterval >= 0.3){
			int currentX;
			currentX = gearToothCounter->Get();
			currentSpeed = (currentX- xIntial)/checkInterval;
			newInterval = 1;
		}
	}
}
void Robot2489::Autonomous(void)
{
	myRobot->SetSafetyEnabled(true);

	double error = 0.0;
	double setpoint = 200.0;
	double PID_Proportional = 0.0000001;
	double input = 0.7;
	
	timer->Start();

	while(IsAutonomous()) {

		UpdateGearToothVelocity();
		error = setpoint - currentSpeed;
		input += error*PID_Proportional;
		bottomTurretWheel->Set(-input);
			
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "E: %f", error);
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "speed: %f", currentSpeed);
		screen->PrintfLine(DriverStationLCD::kUser_Line4, "input: %f", input);
		screen->UpdateLCD();

		if (fabs(error) < 5.0) {
			conveyorBeltMotor->Set(0.5);
		}
		else {
			conveyorBeltMotor->Set(0.0);
		}

		m_watchdog.Feed();
		Wait(0.005);
	}
	/*
	//initial turrent wheel speeds
	bottomWheelValue = -1.0;
	topWheelValue = 1.0;
	
	leftValue = 0.2;
	rightValue = 0.2;
	double error = 0.0;
	
	double time = 0.0;
	timer->Reset();
	timer->Start();
	
	ofstream AutonomousOutput;
	AutonomousOutput.open("autonomous.txt");
	DriverStation *ds = DriverStation::GetInstance();
	DriverStationEnhancedIO *dseio = &ds->GetEnhancedIO();
	
	if(IsAutonomous())
	{
		
		screen->PrintfLine(DriverStationLCD::kUser_Line1, "Jigglybot: Autonomous");
		screen->UpdateLCD();
		
		//start encoders
		leftEncoder->Start();
		rightEncoder->Start();
		
		//zero encoders
		leftEncoder->Reset();
		rightEncoder->Reset();
		
		//Autonomous attempt using only encoder readings
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "Attempt 1");
		screen->UpdateLCD();
		
		myRobot->TankDrive(-0.2, -0.2);
		Wait(1);
		myRobot->TankDrive(0.0, 0.0);
		
		while (IsAutonomous()){
			if ((leftEncoder->GetDistance() < 12.0) && (rightEncoder->GetDistance() < 12.0)){
				myRobot->TankDrive(-0.2, -0.2);
			}
			else{
				myRobot->TankDrive(0.00, 0.00);
				break;
			}
			Wait(0.005);

			screen->PrintfLine(DriverStationLCD::kUser_Line5,"distance traveled %f", rightEncoder->GetDistance());
			screen->PrintfLine(DriverStationLCD::kUser_Line6,"distance traveled %f", leftEncoder->GetDistance());
			screen->UpdateLCD();
			
			if ((timer->Get() - time) > 1.0){
				time = timer->Get();
				AutonomousOutput << timer->Get() << ": "
								 << leftEncoder->GetDistance() << ", "
								 << rightEncoder->GetDistance() << "\r\n";
			}
		}
		
		//Another attempt at moving (moving straight, a foot forward)
		if (!dseio->GetDigital(11)){
		//zero encoders
		leftEncoder->Reset();
		rightEncoder->Reset();
		
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "Attempt 2");
		screen->UpdateLCD();
		myRobot->TankDrive(0.2, 0.2);
		while(IsAutonomous() && (leftEncoder->GetDistance() < 12.0) && (rightEncoder->GetDistance() < 12.0)){
			CorrectDrive();
			myRobot->TankDrive(-correctedLeftValue, -correctedRightValue);
			Wait(0.005);
		}
		}
		//Autonomous attempt using PID loops
		else if(!dseio->GetDigital(7)){
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "Attempt 3");
		screen->UpdateLCD();
		MoveAuton(-12);
		AutonomousOutput << "\r\n" << "PID loop" << "\r\n";
		while (IsAutonomous()){
			
			screen->PrintfLine(DriverStationLCD::kUser_Line5,"PID On");
			screen->PrintfLine(DriverStationLCD::kUser_Line6,"Error: %f", leftMotorController->GetError());
			screen->UpdateLCD();
			
			if ((timer->Get() - time) > 1.0){
				time = timer->Get();
				AutonomousOutput << timer->Get() << ": "
				                 << leftMotorController->GetError() << ", "
				                 << rightMotorController->GetError() << "\r\n";
			}
			
			if(leftMotorController->OnTarget() && rightMotorController->OnTarget()) break;
			Wait(0.005);
		}
		leftMotorController->Disable();
		rightMotorController->Disable();
		}
		
	}	*/
}
